package Model.IntermittentCommunicationModel;

import org.ejml.simple.SimpleMatrix;

/**
 * 基地船
 *
 * @author 17676
 * @date 2022/06/13
 */
public class BaseShip {

    public static int TotalShipNumber = 0;
    public int shipNum;

    public int NowShipPositionInList;

    /**
     * 船舶类型
     */
    public ShipType shipType = ShipType.follower; // 默认船类型为follower

    /**
     * 与领导
     */
    public double distanceWithLeader;
    /**
     * 与追随者
     */
    public double distanceWithFollower;

    /*
        绕z轴旋转就是艏摇(也叫平摇,就是穿在水面打转)
    */
    public double n; // 大地坐标系下的北方向位置
    public double e; // 东方向位置
    public double ψ; // 艏摇角
    public SimpleMatrix ηMatrix; // η=[n,e,ψ]T

    // 大地坐标系下的初始位置，仅用于计算σij
    public double nf; // 大地坐标系下的北方向位置
    public double ef; // 东方向位置
    public double ψf; // 艏摇角
    public SimpleMatrix ηfMatrix; // η=[n,e,ψ]T

    // 大地坐标系下的速度
    public double ns; // 大地坐标系下的北方向位置
    public double es; // 东方向位置
    public double ψs; // 艏摇角
    public SimpleMatrix ηsMatrix; // η=[n,e,ψ]T

    // 大地坐标系下的加速度
    public double nst; // 大地坐标系下的北方向位置
    public double est; // 东方向位置
    public double ψst; // 艏摇角
    public SimpleMatrix ηstMatrix; // η=[n,e,ψ]T

    /*
        船舶的艏-艉（前后）方向称纵向，用X来表示。
        左-右舷（左右）方向称横向，用Y来表示。
        船的上甲板-船舱底（上下）方向称垂直方向，用Z来表示。
        前后方向的晃动（窜动、荡）称为纵荡，左右方向的晃动（窜动、荡）称为横荡，绕船体垂直轴的旋转振荡运动称为艏摇
     */
    public double u; // 为船舶在船体坐标系下的纵荡
    public double v; // 横荡
    public double r; // 艏摇角速度
    public SimpleMatrix vMatrix; // v=[u,v,r]T

    public double τu;
    public double τv;
    public double τr;
    public SimpleMatrix τMatrix; // τ=[τu,τv,τr]T，为控制输入向量

    public double τbu;
    public double τbv;
    public double τbr;
    public SimpleMatrix τbMatrix; // τb=[τbu,τbv,τbr]T，为包含海浪、风等的时变外部干扰，且假设干扰是有界并连续的。

    //  ideal position
    public double idealPositionN; //
    public double idealPositionE;
    public double idealPositionψ;
    public SimpleMatrix idealPositionMatrix; // idealPositionMatrix=[idn,ide,idψ]T，为该船 与 虚拟领航船的相对位置

    /**
     * 领航船的构造函数
     */
    public BaseShip(
            double n,
            double e,
            double ψ
    ) {
        shipNum = TotalShipNumber++;
        this.shipType = ShipType.leader;

        setActualPosition(n, e, ψ); // 设置实际位置

        // 初始化 大地坐标系下的速度矩阵
        this.ns = 50;
        this.es = 50;
        this.ψs = 0;
        ηsMatrix = new SimpleMatrix(new double[][] {{ns}, {es}, {ψs}});

        double[][] RData = {
                {Math.cos(ψ), -Math.sin(ψ), 0},
                {Math.sin(ψ), Math.cos(ψ), 0},
                {0, 0, 1}
        };
        SimpleMatrix R = new SimpleMatrix(RData);
        R = R.invert(); // R为坐标变换矩阵，求出其逆矩阵，左乘于大地坐标系下的速度矩阵，即可得出初始vMatrix矩阵
        this.vMatrix = R.mult(ηsMatrix); // v=[u,v,r]T
        this.u = vMatrix.get(0, 0);
        this.v = vMatrix.get(1, 0); // 横荡
        this.r = vMatrix.get(2, 0); // 艏摇角速度

        // 初始化船的环境影响矩阵为初始t的值
        this.τbu = 0;
        this.τbv = 1200;
        this.τbr = 100;
        double [][] τbMatrixData = {{τbu}, {τbv}, {τbr}};
        this.τbMatrix = new SimpleMatrix(τbMatrixData);

    }

    /**
     * 跟随船的构造函数，需要声明船的初始化数据
     * @param n              n
     * @param e              e
     * @param ψ              ψ
     * @param idealPositionN 理想positionn
     * @param idealPositionE 理想positione
     * @param idealPositionψ 理想positionψ
     */
    public BaseShip(double n,
                    double e,
                    double ψ,

                    double idealPositionN,
                    double idealPositionE,
                    double idealPositionψ
                    ) {
        // 初始船的位置
        this.n = n;
        this.e = e;
        this.ψ = ψ;
        double [][] ηM = {{n}, {e}, {ψ}};
        ηMatrix = new SimpleMatrix(ηM);

        // 存储船的初始位置
        this.nf = n;
        this.ef = e;
        this.ψf = ψ;
        double [][] ηfM = {{n}, {e}, {ψ}};
        ηfMatrix = new SimpleMatrix(ηfM);

        // 初始船的速度为0
        this.ns = 0; // 大地坐标系下的北方向位置
        this.es = 0; // 东方向位置
        this.ψs = 0; // 艏摇角
        this.ηsMatrix = new SimpleMatrix(new double[][] {{ns}, {es}, {ψs}}); // η=[n,e,ψ]T

        // 初始船的加速度为0
        this.nst = 0; // 大地坐标系下的北方向位置
        this.est = 0; // 东方向位置
        this.ψst = 0; // 艏摇角
        this.ηstMatrix = new SimpleMatrix(new double[][] {{nst}, {est}, {ψst}}); // η=[n,e,ψ]T

        // 初始船的移动轨迹方程为0
        this.u = 0;
        this.v = 0; // 横荡
        this.r = 0; // 艏摇角速度
        double [][] vM = {{u}, {v}, {r}};
        this.vMatrix = new SimpleMatrix(vM); // v=[u,v,r]T

        // 初始船与虚拟目标的理想相对位置
        this.idealPositionN = idealPositionN;
        this.idealPositionE = idealPositionE;
        this.idealPositionψ = idealPositionψ;
        double [][] idealPositionM = {{idealPositionN}, {idealPositionE}, {idealPositionψ}};
        idealPositionMatrix = new SimpleMatrix(idealPositionM);

        // 初始化船的控制矩阵为0
        this.τu = 0;
        this.τv = 0;
        this.τr = 0;
        double [][] τMatrixData = {{τu}, {τv}, {τr}};
        this.τMatrix = new SimpleMatrix(τMatrixData);

        // 初始化船的环境影响矩阵为初始t的值
        this.τbu = 0;
        this.τbv = 1200;
        this.τbr = 100;
        double [][] τbMatrixData = {{τbu}, {τbv}, {τbr}};
        this.τbMatrix = new SimpleMatrix(τbMatrixData);

        // 设置船的类型为跟随者
        shipNum = TotalShipNumber++;
        this.shipType = ShipType.follower;
    }

    /**
     * 设置实际位置
     * @param n n
     * @param e e
     * @param ψ ψ
     */
    public void setActualPosition(
            double n,
            double e,
            double ψ
    ) {
        // 初始船的位置
        this.n = n;
        this.e = e;
        this.ψ = ψ;
        double [][] ηM = {{n}, {e}, {ψ}};
        ηMatrix = new SimpleMatrix(ηM);
    }

    // 下列属性对于领航船均无意义
    //  当前时钟周期的计船的速度，船将会以该速度运行一个时钟周期
//    // TODO: 每次运动周期后都需要保存最后计算得到的速度到这，最后预期该速度与ηMatrix中数据相同
//    public double NS = 0;
//    public double ES = 0;
//    public double ψS = 0;
//    //  上一个时钟周期的计算瞬间时的船的速度
//    // TODO: 每次运动周期更新速度前，将前一个周期的速度保存在这里，最后预期该速度与NS……等中数据相同
//    public double lastTimeNS = 0;
//    public double lastTimeES = 0;
//    public double lastTimeψS = 0;

//    public static double timeTampLength = 1; // 时间片段长度，用于计算加速度
    public static double nowTime = 0; // 初始时间为0

    /**
     * 得到领航船的位置
     * 即： η0
     * @param time 时间
     * @return {@link SimpleMatrix}
     */
    public SimpleMatrix getLeaderPosition(double time) {
        // TODO: 该处函数不固定，需要根据实际情况再进行修改
//        double[][] data = {
//                {50 * time},
//                {50 * Math.sin(time)},
//                {0}
//        };
        // TODO: 随意设置后的新函数
        double[][] data = {
                {50 * Math.cos(time)},
                {50 * Math.sin(time)},
                {0}
        };
        if (time >= 2*Math.PI) {
            data = new double[][]{
                    {50 * (time- 2*Math.PI + 1)},
                    {50 * (time- 2*Math.PI)},
                    {0}
            };
        }
        return new SimpleMatrix(data);
    }

    /**
     * 得到领航船的速度
     * 即： ˙η0
     * @param time 时间
     * @return {@link SimpleMatrix}
     */
    public SimpleMatrix getLeaderSpeed(double time) {
        // TODO: 该处函数不固定，需要根据实际情况再进行修改
        System.out.println("当前时间" + time);
//        double[][] data = {
//                {50},
//                {50 * Math.cos(time)},
//                {0}
//        };
        // TODO: 新函数
        double[][] data = {
                {-50 * Math.sin(time)},
                {50 * Math.cos(time)},
                {0}
        };
        if (time >= 2*Math.PI) {
            data = new double[][]{
                    {50},
                    {50},
                    {0}
            };
        }
        return new SimpleMatrix(data);
    }


    /**
     * 得到领航船的加速度
     * 即： ˙˙η0
     * @param time 时间
     * @return {@link SimpleMatrix}
     */
    public SimpleMatrix getLeaderAccelerateSpeed(double time) {
        // TODO: 该处函数不固定，需要根据实际情况再进行修改
//        double[][] data = {
//                {0},
//                {-50 * Math.sin(time)},
//                {0}
//        };
        // TODO: 新函数
        double[][] data = {
                {-50 * Math.cos(time)},
                {-50 * Math.sin(time)},
                {0}
        };
        if (time >= 2*Math.PI) {
            data = new double[][]{
                    {0},
                    {0},
                    {0}
            };
        }
        return new SimpleMatrix(data);
    }

    /**
     * 获取当前位置
     *
     * @return {@link SimpleMatrix}
     */
    public SimpleMatrix getCurrentPosition() {
        switch (this.shipType) {
            case leader:
                return getLeaderPosition(nowTime);
            case follower:
                return this.ηMatrix;
            default:
                return null;
        }
    }

    /**
     * 即： ˙η
     * 根据船的类型不同采取不同的方法
     * 获取该船的速度
     * @return {@link SimpleMatrix}
     */
    public SimpleMatrix getTansPositionSpeed() {
        switch (this.shipType) {
            case leader:
                return getLeaderSpeed(nowTime);
            case follower:
                return this.ηsMatrix;
//                return ShipController.getTansPosition(this.ψ, this.vMatrix);
            default:
                return null;
        }
    }

    /**
     * 获取当前时钟周期的加速度，根据船的类型分别判断如何计算加速度
     * 即： ˙˙η
     * @return double
     */
    public SimpleMatrix getAccelerateSpeed() {
        switch (this.shipType) {
            case leader:
                return getLeaderAccelerateSpeed(nowTime);
            case follower:
                return this.ηstMatrix;
            default:
                return null;
        }
    }




//    /**
//     * 设置vmatrix速度
//     * 设定船体坐标系下的纵荡、横荡和艏摇角速度速度
//     * @param vM 虚拟
//     */
//    public void setVMatrixSpeed(
//            SimpleMatrix vM
//    ) {
//        // 初始化 大地坐标系下的速度矩阵
//        this.u = vM.get(0, 0);
//        this.v = vM.get(1, 0);
//        this.r = vM.get(2, 0);
//        vMatrix = new SimpleMatrix(new double[][] {{u}, {v}, {r}});
//
//        double[][] RData = {
//                {Math.cos(ψ), -Math.sin(ψ), 0},
//                {Math.sin(ψ), Math.cos(ψ), 0},
//                {0, 0, 1}
//        };
//        SimpleMatrix R = new SimpleMatrix(RData);
//        // 初始化 大地坐标系下的速度矩阵
//        ηsMatrix = R.mult(vMatrix);
//        this.ns = ηsMatrix.get(0, 0);
//        this.es = ηsMatrix.get(1, 0);
//        this.ψs = ηsMatrix.get(2, 0);
//    }

    /**
     * 设置控制矩阵
     * @param v v
     */
    public void setControlMatrix(
            SimpleMatrix v
    ) {
        this.τu = v.get(0, 0);
        this.τv = v.get(1, 0);
        this.τr = v.get(2, 0);
        this.τMatrix = new SimpleMatrix(new double[][] {{τu}, {τv}, {τr}}); // τ=[τu,τv,τr]T，为控制输入向量
    }

    /**
     * 设置位置
     * @param v v
     */
    public void setPosition(
            SimpleMatrix v
    ) {
//        System.out.println("获取到的位置" + v);
        this.ηMatrix = v;
        this.n = ηMatrix.get(0, 0);
        this.e = ηMatrix.get(1, 0);
        this.ψ = ηMatrix.get(2, 0);
//        System.out.println("设置的位置" + ηMatrix);
    }

    /**
     * 设置速度与矩阵
     *
     * @param v v
     */
    public void setSpeedWithMatrix(
            SimpleMatrix v
    ) {
        // 初始化 大地坐标系下的速度矩阵
//        System.out.println("获取到的速度矩阵" + v);
        ηsMatrix = v;
        this.ns = ηsMatrix.get(0, 0);
        this.es = ηsMatrix.get(1, 0);
        this.ψs = ηsMatrix.get(2, 0);
//        System.out.println("设置的的速度矩阵" + ηsMatrix);

//        double[][] RData = {
//                {Math.cos(ψ), -Math.sin(ψ), 0},
//                {Math.sin(ψ), Math.cos(ψ), 0},
//                {0, 0, 1}
//        };
//        SimpleMatrix R = new SimpleMatrix(RData);
//        R = R.invert(); // R为坐标变换矩阵，求出其逆矩阵，左乘于大地坐标系下的速度矩阵，即可得出初始vMatrix矩阵
//        this.vMatrix = R.mult(ηsMatrix); // v=[u,v,r]T
//        this.u = vMatrix.get(0, 0);
//        this.v = vMatrix.get(1, 0); // 横荡
//        this.r = vMatrix.get(2, 0); // 艏摇角速度
    }

    /**
     * 设置加速度
     *
     * @param as
     */
    public void setAccelerateSpeed(
            SimpleMatrix as
    ) {
//        System.out.println("获取到的加速度矩阵" + as);
//        this.nst = as.get(0, 0);
//        this.est = as.get(1, 0);
//        this.ψst = as.get(2, 0);
//        this.ηstMatrix  = new SimpleMatrix(new double[][] {{n}, {e}, {ψ}});
        this.ηstMatrix = as;
        this.nst = ηstMatrix.get(0, 0);
        this.est = ηstMatrix.get(1, 0);
        this.ψst = ηstMatrix.get(2, 0);
    }

    /**
     * 移动
     */
    public void LeaderMove() {
        this.setPosition(getLeaderPosition(nowTime));
        this.setSpeedWithMatrix(getLeaderSpeed(nowTime));
        this.setAccelerateSpeed(getLeaderAccelerateSpeed(nowTime));
    }


    public String toString() {
        String position = "船的大地坐标系下的位置：" + this.ηMatrix.toString() + " ";
        String speed = "船:" + this.shipNum + " 的大地坐标系下的速度：" + this.ηsMatrix.toString() + " ";
        String speed2 = "船" + this.shipNum + " 的在船体坐标系下的速度：" + this.vMatrix.toString() + " ";
        return "船：" + this.shipNum + " 的数据：" + position + speed + speed2;
    }

}
